//
// Created by Wirano on 2020/11/9.
//

#include "Ano_Imu.h"
#include "Ano_Math.h"
#include "Ano_Filter.h"
#include "Ano_DT.h"
//#include "ANO_RC.h"

/*参考坐标，定义为ANO坐标*

俯视，机头方向为x正方向
     +x
     |
 +y--|--
     |
//这是一个典型的右手系
*/

/// \brief 世界坐标平面XY转平面航向坐标XY
/// \param w 世界坐标系数组
/// \param ref_ax   [X]:相当于cos(theta),[Y]:相当于sin(theta),theta为飞机的航向角，就是即X轴正方向和飞机航向方向直接的夹角
/// \param h    航线坐标系数组
void w2h_2d_trans(float w[2],float ref_ax[VEC_XYZ],float h[2])
{
    h[X] =  w[X] *  ref_ax[X]  + w[Y] *ref_ax[Y];
    h[Y] =  w[X] *(-ref_ax[Y]) + w[Y] *ref_ax[X];

}
/// \brief 平面航向坐标XY转世界坐标平面XY
/// \param h 航线坐标系数组
/// \param ref_ax 参考轴
/// \param w 世界坐标系数组
void h2w_2d_trans(float h[2],float ref_ax[VEC_XYZ],float w[2])
{
    w[X] = h[X] *ref_ax[X] + h[Y] *(-ref_ax[Y]);
    w[Y] = h[X] *ref_ax[Y] + h[Y] *  ref_ax[X];

}

float att_matrix[3][3]; //必须由姿态解算算出该矩阵
/// \brief 载体坐标转世界坐标
/// \param a 载体坐标系
/// \param w 世界坐标系
void a2w_3d_trans(float a[VEC_XYZ],float w[VEC_XYZ])
{
    for(uint8_t i = 0;i<3;i++)
    {
        float temp = 0;
        for(uint8_t j = 0;j<3;j++)
        {

            temp += a[j] *att_matrix[i][j];
        }
        w[i] = temp;
    }
}

//float mag_yaw_calculate(float dT,float mag_val[VEC_XYZ],float g_z_vec[VEC_XYZ],float h_mag_val[VEC_XYZ])//
//{

//	vec_3dh_transition(g_z_vec, mag_val, h_mag_val);

//	return (fast_atan2(h_mag_val[Y], h_mag_val[X]) *57.3f) ;// 	
//}



#define USE_MAG
#define USE_LENGTH_LIM



_imu_st imu_data =  {1,0,0,0,
                     {0,0,0},
                     {0,0,0},
                     {0,0,0},
                     {0,0,0},
                     {0,0,0},
                     {0,0,0},
                     0,0,0};

static float vec_err[VEC_XYZ];
static float vec_err_i[VEC_XYZ];
static float q0q1,q0q2,q1q1,q1q3,q2q2,q2q3,q3q3,q1q2,q0q3;//q0q0,				
static float mag_yaw_err,mag_err_dot_prudoct,mag_val_f[VEC_XYZ];
static float imu_reset_val;

static uint16_t reset_cnt;

_imu_state_st imu_state = {1,1,1,1,1,1,1,1};

static float mag_2d_w_vec[2][2] = {{1,0},{1,0}};//地理坐标中，水平面磁场方向恒为南北 (1,0)

float imu_test[3];
/// \brief 姿态的计算，更新和融合
/// \param dT 任务周期
/// \param state imu状态
/// \param gyr  陀螺仪数据
/// \param acc  加速度计数据
/// \param mag_val 磁力计数据
/// \param imu  imu结构体
void IMU_update(float dT,_imu_state_st *state,float gyr[VEC_XYZ], int32_t acc[VEC_XYZ],int16_t mag_val[VEC_XYZ],_imu_st *imu)
{
//	const float kp = 0.2f,ki = 0.001f;
//	const float kmp = 0.1f;

    static float kp_use = 0,ki_use = 0,mkp_use = 0;

    float acc_norm_l,acc_norm_l_recip,q_norm_l;

    float acc_norm[VEC_XYZ];

    float d_angle[VEC_XYZ];




//		q0q0 = imu->w * imu->w;							
    q0q1 = imu->w * imu->x;
    q0q2 = imu->w * imu->y;
    q1q1 = imu->x * imu->x;
    q1q3 = imu->x * imu->z;
    q2q2 = imu->y * imu->y;
    q2q3 = imu->y * imu->z;
    q3q3 = imu->z * imu->z;
    q1q2 = imu->x * imu->y;
    q0q3 = imu->w * imu->z;


    if(state->obs_en)
    {
        //计算机体坐标下的运动加速度观测量。坐标系为北西天
        for(uint8_t i = 0;i<3;i++)
        {
            int32_t temp = 0;
            for(uint8_t j = 0;j<3;j++)
            {

                temp += imu->obs_acc_w[j] *att_matrix[j][i];//t[i][j] 转置为 t[j][i]
            }
            imu->obs_acc_a[i] = temp;

            imu->gra_acc[i] = acc[i] - imu->obs_acc_a[i];
        }
    }
    else
    {
        for(uint8_t i = 0;i<3;i++)
        {
            imu->gra_acc[i] = acc[i];
        }
    }
    //
    acc_norm_l_recip = my_sqrt_reciprocal(my_pow(imu->gra_acc[X]) + my_pow(imu->gra_acc[Y]) + my_pow(imu->gra_acc[Z]));
    acc_norm_l = safe_div(1,acc_norm_l_recip,0);

    // 加速度计的读数，单位化。
    for(uint8_t i = 0;i<3;i++)
    {
        acc_norm[i] = imu->gra_acc[i] *acc_norm_l_recip;
    }




    // 载体坐标下的x方向向量，单位化。
    att_matrix[0][0] = imu->x_vec[X] = 1 - (2*q2q2 + 2*q3q3);
    att_matrix[0][1] = imu->x_vec[Y] = 2*q1q2 - 2*q0q3;
    att_matrix[0][2] = imu->x_vec[Z] = 2*q1q3 + 2*q0q2;

    // 载体坐标下的y方向向量，单位化。
    att_matrix[1][0] = imu->y_vec[X] = 2*q1q2 + 2*q0q3;
    att_matrix[1][1] = imu->y_vec[Y] = 1 - (2*q1q1 + 2*q3q3);
    att_matrix[1][2] = imu->y_vec[Z] = 2*q2q3 - 2*q0q1;

    // 载体坐标下的z方向向量（等效重力向量、重力加速度向量），单位化。
    att_matrix[2][0] = imu->z_vec[X] = 2*q1q3 - 2*q0q2;
    att_matrix[2][1] = imu->z_vec[Y] = 2*q2q3 + 2*q0q1;
    att_matrix[2][2] = imu->z_vec[Z] = 1 - (2*q1q1 + 2*q2q2);

    //水平面方向向量
    float hx_vec_reci = my_sqrt_reciprocal(my_pow(att_matrix[0][0]) + my_pow(att_matrix[1][0]));
    imu->hx_vec[X] = att_matrix[0][0] *hx_vec_reci;
    imu->hx_vec[Y] = att_matrix[1][0] *hx_vec_reci;


    // 计算载体坐标下的运动加速度。(与姿态解算无关)
    for(uint8_t i = 0;i<3;i++)
    {
        imu->a_acc[i] = (int32_t)(acc[i] - 981 *imu->z_vec[i]);
    }


    //计算世界坐标下的运动加速度。坐标系为北西天
    for(uint8_t i = 0;i<3;i++)
    {
        int32_t temp = 0;
        for(uint8_t j = 0;j<3;j++)
        {

            temp += imu->a_acc[j] *att_matrix[i][j];
        }
        imu->w_acc[i] = temp;
    }

    w2h_2d_trans(imu->w_acc,imu_data.hx_vec,imu->h_acc);


    // 测量值与等效重力向量的叉积（计算向量误差）。
    vec_err[X] =  (acc_norm[Y] * imu->z_vec[Z] - imu->z_vec[Y] * acc_norm[Z]);
    vec_err[Y] = -(acc_norm[X] * imu->z_vec[Z] - imu->z_vec[X] * acc_norm[Z]);
    vec_err[Z] = -(acc_norm[Y] * imu->z_vec[X] - imu->z_vec[Y] * acc_norm[X]);


#ifdef USE_MAG //使用磁力计

    //电子罗盘赋值为float矢量
    for(uint8_t i = 0;i<3;i++)
    {
        mag_val_f[i] = (float)mag_val[i];
    }

    if(!(mag_val[X] ==0 && mag_val[Y] == 0 && mag_val[Z] == 0))
    {
        //把载体坐标下的罗盘数据转换到地理坐标下
        a2w_3d_trans(mag_val_f,imu->w_mag);
        //计算方向向量归一化系数（模的倒数）
        float l_re_tmp = my_sqrt_reciprocal(my_pow(imu->w_mag[0]) + my_pow(imu->w_mag[1]));
        //计算南北朝向向量
        mag_2d_w_vec[1][0] = imu->w_mag[0] *l_re_tmp;
        mag_2d_w_vec[1][1] = imu->w_mag[1] *l_re_tmp;
        //计算南北朝向误差(叉乘)，地理坐标中，水平面磁场方向向量应恒为南北 (1,0)
        mag_yaw_err = vec_2_cross_product(mag_2d_w_vec[1],mag_2d_w_vec[0]);
        //计算南北朝向向量点乘，判断同向或反向
        mag_err_dot_prudoct = vec_2_dot_product(mag_2d_w_vec[1],mag_2d_w_vec[0]);
        //若反向，直接给最大误差
        if(mag_err_dot_prudoct<0)
        {
            mag_yaw_err = my_sign(mag_yaw_err) *1.0f;
        }
        //

    }
#endif

    for(uint8_t i = 0;i<3;i++)
    {

#ifdef USE_EST_DEADZONE
        if(state->G_reset == 0 && state->obs_en == 0)
			{
				vec_err[i] = my_deadzone(vec_err[i],0,imu->gacc_deadzone[i]);
			}
#endif

#ifdef USE_LENGTH_LIM
        if(acc_norm_l>1060 || acc_norm_l<900)
        {
            vec_err[X] = vec_err[Y] = vec_err[Z] = 0;
        }
#endif
        //误差积分
        vec_err_i[i] +=  LIMIT(vec_err[i],-0.1f,0.1f) *dT *ki_use;


        // 构造增量旋转（含融合纠正）。	
        //    d_angle[X] = (gyr[X] + (vec_err[X]  + vec_err_i[X]) * kp_use - mag_yaw_err *imu->z_vec[X] *kmp_use *RAD_PER_DEG) * dT / 2 ;
        //    d_angle[Y] = (gyr[Y] + (vec_err[Y]  + vec_err_i[Y]) * kp_use - mag_yaw_err *imu->z_vec[Y] *kmp_use *RAD_PER_DEG) * dT / 2 ;
        //    d_angle[Z] = (gyr[Z] + (vec_err[Z]  + vec_err_i[Z]) * kp_use - mag_yaw_err *imu->z_vec[Z] *kmp_use *RAD_PER_DEG) * dT / 2 ;


#ifdef USE_MAG
        d_angle[i] = (gyr[i] + (vec_err[i]  + vec_err_i[i]) * kp_use + mag_yaw_err *imu->z_vec[i] *mkp_use) * dT / 2 ;
#else
        d_angle[i] = (gyr[i] + (vec_err[i]  + vec_err_i[i]) * kp_use ) * dT / 2 ;
#endif
    }
    // 计算姿态。

    imu->w = imu->w            - imu->x*d_angle[X] - imu->y*d_angle[Y] - imu->z*d_angle[Z];
    imu->x = imu->w*d_angle[X] + imu->x            + imu->y*d_angle[Z] - imu->z*d_angle[Y];
    imu->y = imu->w*d_angle[Y] - imu->x*d_angle[Z] + imu->y            + imu->z*d_angle[X];
    imu->z = imu->w*d_angle[Z] + imu->x*d_angle[Y] - imu->y*d_angle[X] + imu->z;

    q_norm_l = my_sqrt_reciprocal(imu->w*imu->w + imu->x*imu->x + imu->y*imu->y + imu->z*imu->z);
    imu->w *= q_norm_l;
    imu->x *= q_norm_l;
    imu->y *= q_norm_l;
    imu->z *= q_norm_l;



    /////////////////////修正开关///////////////////////////
#ifdef USE_MAG
    if(state->M_fix_en==0)//磁力
    {
        mkp_use = 0;//不修正
        state->M_reset = 0;//罗盘修正不复位，清除复位标记
    }
    else
    {
        if(state->M_reset)//
        {
            //通过增量进行对准
            mkp_use = 10.0f;
            if(mag_yaw_err != 0 && ABS(mag_yaw_err)<0.01f)
            {
                state->M_reset = 0;//误差小于2的时候，清除复位标记
            }
        }
        else
        {
            mkp_use = state->mkp; //正常修正
        }
    }
#endif

    if(state->G_fix_en==0)//重力方向修正
    {
        kp_use = 0;//不修正
    }
    else
    {
        if(state->G_reset == 0)//正常修正
        {
            kp_use = state->gkp;
            ki_use = state->gki;
        }
        else//快速修正，通过增量进行对准
        {
            kp_use = 10.0f;
            ki_use = 0.0f;
//				imu->est_speed_w[X] = imu->est_speed_w[Y] = 0;
//				imu->est_acc_w[X] = imu->est_acc_w[Y] = 0;
//				imu->est_acc_h[X] = imu->est_acc_h[Y] =0;

            //计算静态误差是否缩小
//				imu_reset_val += (ABS(vec_err[X]) + ABS(vec_err[Y])) *1000 *dT;
//				imu_reset_val -= 0.01f;
            imu_reset_val = (ABS(vec_err[X]) + ABS(vec_err[Y]));

            imu_reset_val = LIMIT(imu_reset_val,0,1.0f);

            if((imu_reset_val < 0.02f) && (state->M_reset == 0))
            {
                //计时
                reset_cnt += 2;
                if(reset_cnt>400)
                {
                    reset_cnt = 0;
                    state->G_reset = 0;//已经对准，清除复位标记
                }
            }
            else
            {
                reset_cnt = 0;
            }
        }
    }
}

static float t_temp;
/**
 *  \brief 获取姿态角
 */
void calculate_RPY()
{
    ///////////////////////输出姿态角///////////////////////////////

    t_temp = LIMIT(1 - my_pow(att_matrix[2][0]),0,1);

    //imu_data.pit = asin(2*q1q3 - 2*q0q2)*57.30f;

    if(ABS(imu_data.z_vec[Z])>0.05f)//避免奇点的运算
    {
        imu_data.pit =  fast_atan2(att_matrix[2][0],my_sqrt(t_temp))*57.30f;
        imu_data.rol =  fast_atan2(att_matrix[2][1], att_matrix[2][2])*57.30f;
        imu_data.yaw = -fast_atan2(att_matrix[1][0], att_matrix[0][0])*57.30f;
    }
}

